/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

#include "dmsim.h"
#include "utilities.h"
#include <glib.h>

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
extern bool logging;
extern String logDir;
/*<-End Global Variables-->*/


DMSim::DMSim(String name, int port) : ThreadedPipe(name)
{
  int r;
  
  for (int i=0; i<NUM_DEPLOY_MECHANISMS; i++) {
    DeployMechanism_t dm = mothership.pegs[i];

    r = snprintf(mothership.pegs[i].id, NAME_STRING_LEN, "dm%d", i);
    if (r <= 0) {
      cout << "sprintf failed: %s" << strerror(errno);
      // can't exit...so...?
    }

    mothership.pegs[i].status = DM_LOADED;
  }

  this->port = port;

  status = OFF_STATE;
  printf("status = %X\n", status);

  // ~ bitwise not
  // ^ bitwise xor
}


int DMSim::random_set() {
  FILE* urandom_fd;
  int num, r;

  urandom_fd = fopen("/dev/urandom", "r");
  if ( urandom_fd == NULL ) {
    cout << "error opening urandom" << endl;
  }

  r = fread(&num, 4, 1, urandom_fd);
  if (r != 1) {
    cout << "bad read!" << endl;
    num = 1;
  }

  fclose(urandom_fd);

  return num%3;
}


bool DMSim::init(ShmStruct * shmStructPtr) {
  ThreadedPipe::init(shmStructPtr);

  Client * me = shmPtr->clients.getClient(0);
  shmPtr->clients.subscribe(me, CMD_PTH);

  if (logging)
    log_name = generteNumberedFile(logDir, "_ms_deploy_mechanism");

  // start the tcp thread (for sending daq data)
  g_thread_init(NULL);
  g_thread_create(tcpServer, this, FALSE, NULL);

  return true;
}


void DMSim::update() {
  int val, maxFD;
  fd_set readFDs, writeFDs;                   // fd sets
  struct timeval timeout;

  FD_ZERO(&readFDs);                          // zero read fd set
  FD_ZERO(&writeFDs);                         // zero write fd set
  FD_SET(fifo_FDs[INBOARD_RX],&readFDs);      // add fifo
  FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);     // add fifo

  maxFD = fifo_FDs[INBOARD_RX];
  if(fifo_FDs[INBOARD_TX] > maxFD) maxFD = fifo_FDs[INBOARD_TX];

  timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

  if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
    if( val != 0 ) {
      if (errno != EINTR) {
        perror("Template::update() select");
        execRate = -1;
      } else
        cout << getName() << "::update() - select interrupted" << endl;
    }

    return;
  }

  if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
    int n = 0;
    n=readFifo(INBOARD_RX);
    if(n>0) {
      // parse commands
      if ( !validRead() ) // do some basic error checking
        return;
      
      Command cmd(fifo_msg.data);
      if( !cmd.isValid() ) {
        cout << getName() << "::update() - error, invalid command" << endl << cmd ;
      } else {
        Client * a_client = shmPtr->clients.getClient(fifo_msg.src);
        uint8_t *dataPtr = CMD_DATAPTR(fifo_msg.data);
        switch(cmd.cmd()) {
          case CMD_DEPLOY_PEG: {
            // update the mothership as we see fit
            int peg = ((DMDeploy_t*)dataPtr)->peg;

            // launch if we are loaded!
            if (mothership.pegs[peg].status == DM_LOADED)
              mothership.pegs[peg].status = DM_EMPTY;

            // NOTE: status is used for the deploy mechanism state, NOT
            //  the DS state
            status ^= dm_on_map[peg];

            printf("status (peg %d) = %X\n", peg, status);

            // FIXME: change to write to TCP client
            if (num_connections > 0) {
              send(client_fds[num_connections-1], &status, sizeof(status), 0);
              printf("wrote to NIDAQ @ %d\n", client_fds[0]);
            } else {
              printf("should have wrote to NIDAQ!\n");
            }
            
            break;
          }
          case CMD_DEPLOY_STATUS: {
            // send back the deploy status
            //Mothership_t *ms = (Mothership_t*)dataPtr;
            printf("INBOARD_RX got CMD_DEPLOY_STATUS, doing nothing\n");
          } break;
          case CMD_DEPLOY: {
            // not sure what we should do here...
            printf("INBOARD_RX got CMD_DEPLOY, doing nothing\n");
          } break;
          case CMD_CAPABILITY: {
            publishSim();
          }  break;
          case CMD_STREAM_REQ: {
            StreamReq_t * request = (StreamReq_t*)CMD_DATAPTR(fifo_msg.data);
            printf("INBOARD_RX Stream request [%X] from %s\n", 
              CMD_STREAM_REQ,
              inet_ntoa(request->client));
            if (request->send_data) {
              publish(&fifo_msg.src.s_addr);
              shmPtr->clients.subscribe(a_client, request->id);
            } else {
              shmPtr->clients.unsubscribe(a_client, request->id);
              cout << "resetting MS configuration" << endl;
              for (int i=0; i<NUM_DEPLOY_MECHANISMS; i++) {
                mothership.pegs[i].status = (DMStatus_t)random_set();
              }
            }
          } break;
          default:
            printf("INBOARD_RX got unknown (%X) on INBOARD_RX\n", cmd.cmd());
            break;
        }
      }
    }
  }
}


bool DMSim::validRead() {
  uint16_t length = CMD_SIZE(fifo_msg.data);
  if( length != fifo_msg.size) {
    cout << getName() << "::update() - error, length of command != length of buffer" << endl
      << "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

    if( length < 1000 && length <= fifo_msg.size ){
      if( C_calcCheckSum((uint8_t *)buf) == 0 )
        cout << "\tCMD: is valid" << endl;
      else
        cout << "\tCMD: is NOT valid" << endl;
    }

    return false;
  }

  return true;
}


// arbitrarily publish updates to the mothership status
void DMSim::publishSim() {
  Client *client;

  int i = 0;

  while ((client = shmPtr->clients.getSubscriptionsTo(CMD_DEPLOY_STATUS,i)) != NULL) {
    publish(&shmPtr->clients.getAddr(client).s_addr);
    i++;
  }
}

void DMSim::publish(in_addr_t *dest) {
  CMD_CMD((fifo_msg.data)) = CMD_DEPLOY_STATUS;
  CMD_NUM((fifo_msg.data)) = sizeof(Mothership_t);
  memcpy(CMD_DATAPTR(fifo_msg.data), &mothership, sizeof(Mothership_t));
  CMD_CSUM((fifo_msg.data)) = 0;
  CMD_CSUM((fifo_msg.data)) = C_calcCheckSum(fifo_msg.data);

  fifo_msg.size = CMD_SIZE(fifo_msg.data);
  fifo_msg.dest.s_addr = *dest;
  fifo_msg.src.s_addr = shmPtr->clients.getAddr(shmPtr->clients.getClient(0)).s_addr;
  writeFifo(INBOARD_TX);
}


void* DMSim::tcpServer(void *data) {
  struct sockaddr_in client;
  struct sockaddr_in server_info;
  int len = sizeof(client);
  int r, servSocket;
  DMSim *me = (DMSim*)data;

  servSocket = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP);
  if (servSocket < 0) {
    printf("socket failed: %s, not starting tcp server\n", strerror(errno));
    return NULL;
  }

  server_info.sin_family = AF_INET;
  server_info.sin_addr.s_addr = inet_addr("127.0.0.1");
  server_info.sin_port = htons(me->port);

  for (int i=0; i<4; i++)
    printf("%d.", ((uint8_t*)&(server_info.sin_addr.s_addr))[i]);
  printf("\n");

  r = bind(servSocket, (struct sockaddr*)&server_info, sizeof(server_info));
  if (r < 0) {
    printf("bind failed: %s, not starting tcp server\n", strerror(errno));
    return NULL;
  }

  r = listen(servSocket, MAX_CONNECTIONS);
  if (r < 0) {
    printf("listen failed: %s, not starting tcp server\n", strerror(errno));
    return NULL;
  }

  // spin, blocking on incoming connections
  while (1) {
    printf("waiting for connections (servSocket is %d)\n", servSocket);
    r = accept(servSocket, (struct sockaddr*)&client, (socklen_t*)&len);
    if (r < 0) {
      printf("accept error: %s\n", strerror(errno));
    } else {
      me->client_fds[me->num_connections] = r;
      printf("accepted a connection (%d)\n", me->client_fds[me->num_connections]);
      me->num_connections = me->num_connections + 1;
    }
  }
}

void DMSim::hexDump(char *buf, int len) {
  for (int i =0; i < len; i++)
    printf("%X ", buf[i]);
  printf("\n");
}

